﻿using UnityEngine;
using System;
using System.Collections;
using Kinect2;

public class CoordinateMappingBasic : MonoBehaviour {

    KinectSensor kinectSensor;
    CoordinateMapper coordinateMapper;
    MultiSourceFrameReader multiFrameSourceReader;
    DepthSpacePoint[] colorMappedToDepthPoints;
    byte[] bitmapData;
    Texture2D texture;
    GameObject status;

	void Start ()
    {
        kinectSensor = KinectSensor.GetDefault();
            
        multiFrameSourceReader = kinectSensor.OpenMultiSourceFrameReader(FrameSourceTypes.Depth | FrameSourceTypes.Color | FrameSourceTypes.BodyIndex);
        multiFrameSourceReader.FrameArrived += Reader_MultiSourceFrameArrived;

        coordinateMapper = kinectSensor.CoordinateMapper;

        FrameDescription colorFrameDescription = kinectSensor.ColorFrameSource.FrameDescription;

        int colorWidth = colorFrameDescription.Width;
        int colorHeight = colorFrameDescription.Height;
        bitmapData = new byte[colorWidth * colorHeight * 4];

        colorMappedToDepthPoints = new DepthSpacePoint[colorWidth * colorHeight];
        texture = new Texture2D(colorWidth, colorHeight, TextureFormat.BGRA32, false);

        kinectSensor.IsAvailableChanged += Sensor_IsAvailableChanged;
        kinectSensor.Open();

        InitializeComponent();
	}
	
	void Update () 
    {
        EventRouter.Check();
	}

	void OnApplicationQuit()
	{
        if (multiFrameSourceReader != null)
            multiFrameSourceReader.Dispose();
        multiFrameSourceReader = null;

        if (kinectSensor != null)
            kinectSensor.Close();
        kinectSensor = null;
	}

    void InitializeComponent()
    {
        GameObject color = GameObject.Find("color");
        color.guiTexture.texture = texture;
        status = GameObject.Find("status");
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "No ready Kinect found!";
    }

    void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
    {
        int depthWidth = 0;
        int depthHeight = 0;

        DepthFrame depthFrame = null;
        ColorFrame colorFrame = null;
        BodyIndexFrame bodyIndexFrame = null;

        MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
        if (multiSourceFrame == null)
            return;

        try
        {
            depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
            colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
            bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();


			if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null) )
			{
				if (depthFrame == null)
					Debug.Log("depth");

				if (colorFrame == null)
					Debug.Log("color");

				if (bodyIndexFrame == null)
					Debug.Log("bodyindex");
				
				return;
			}

            FrameDescription depthFrameDescription = depthFrame.FrameDescription;
            depthWidth = depthFrameDescription.Width;
            depthHeight = depthFrameDescription.Height;

            using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
            {
                coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
                    depthFrameData.UnderlyingBuffer,
                    depthFrameData.Size,
                    colorMappedToDepthPoints);
            }

            depthFrame.Dispose();
            depthFrame = null;
			
            colorFrame.CopyConvertedFrameDataToArray(bitmapData, ColorImageFormat.Rgba);
            colorFrame.Dispose();
            colorFrame = null;

            using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer())
            {
                unsafe
                {
                    byte* bodyIndexDataPointer = (byte*)bodyIndexData.UnderlyingBuffer;

                    int colorMappedToDepthPointCount = this.colorMappedToDepthPoints.Length;

                    fixed (DepthSpacePoint* colorMappedToDepthPointsPointer = colorMappedToDepthPoints)
                    {

                        for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
                        {
                            float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
                            float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;

                            if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
                                !float.IsNegativeInfinity(colorMappedToDepthY))
                            {
                                int depthX = (int)(colorMappedToDepthX + 0.5f);
                                int depthY = (int)(colorMappedToDepthY + 0.5f);

                                if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
                                {
                                    int depthIndex = (depthY * depthWidth) + depthX;

                                    if (bodyIndexDataPointer[depthIndex] != 0xff)
                                    {
                                        continue;
                                    }
                                }
                            }
                            bitmapData[colorIndex * 4 + 3] = 0; 
                        }
                    }
                    ProcessColor(bitmapData);
                }
			}
			bodyIndexFrame.Dispose();
			bodyIndexFrame = null;
        }
		catch {}
        finally
        {
            if (depthFrame != null)
                depthFrame.Dispose();

            if (colorFrame != null)
                colorFrame.Dispose();

            if (bodyIndexFrame != null)
                bodyIndexFrame.Dispose();
        }
    }

    void Sensor_IsAvailableChanged(object sender, IsAvailableChangedEventArgs e)
    {
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "Kinect not available!";
    }

    void ProcessColor(byte[] data)
    {
        int width = texture.width;
        int height = texture.height;
		byte[] data2 = new byte[width * height * 4];
        for (int j = 0; j < height; j++)
        {
            int dst = width * 4 * (height - 1 - j);
            int src = width * 4 * j;
            Array.Copy(data, src, data2, dst, width * 4);
        }
        texture.LoadRawTextureData(data2);
        texture.Apply();
    }

}
